% Q_learning算法求解最优路径
close all;clear;clc;
% 初始化参数
rng(1234);
mazeHeight = 50;% 迷宫长度
mazeWidth = 50;% 迷宫宽度
numStates = mazeHeight * mazeWidth; % 迷宫有2500(mazeHeight*mazeWidth)个状态（格子）  
numActions = 8; % 智能体可以执行的动作数量：上、下、左、右、右上、右下、左上、左下
alphaMin = 0.2; % 学习率min
alphaMax = 0.8; % 学习率max
gamma = 0.99; % 折扣因子
epsilonMin = 0.01; % 探索率min
epsilonMax = 0.3; % 探索率max
maxEpochs = 10000; % 最大训练轮数
maxSteps = 500; % 最大步数
bestSteps = maxSteps;% 最少步数
Q = zeros(numStates, numActions);% 初始化Q表
bestQ = Q;
% 运行选项
showFlag = 0;% 显示结果
recordFlag = 0;% 保存结果
% 定义迷宫环境  
% 使用0表示墙壁，1表示可通行区域，2表示起点，3表示终点，4表示途经点  
maze = Maze(mazeWidth,mazeHeight);
mazeDataInit = maze.generate();
mazeMapInit = maze.mazeMap;
maze.draw();
% 获取开始和结束索引  
startState = find(maze.mazeData(:) == 2);
goalState = find(maze.mazeData(:) == 3);
transitPointState = find(maze.mazeData(:) == 4);
% 定义动作(row+1=向右、col+1=向下)
actions = [0 -1; 0 1; -1 0; 1 0;1 -1;1 1;-1 -1;-1 1]; % 上、下、左、右、右上、右下、左上、左下
% 保存为.mp4视频
if recordFlag && showFlag
    v = VideoWriter('./result/Qlearning_Diagonal1.mp4', 'MPEG-4');
    v.FrameRate = 60;
    open(v);
end
% 训练Q-learning
for epoch = 1:maxEpochs
    if mod(epoch,1000)==0 && showFlag
        clf;
        maze.mazeMap = mazeMapInit;
        maze.draw();
        maze.mazeMap = [[0 0 1;0 1 1];mazeMapInit];
        drawMazeFlag = 1;
        if recordFlag && showFlag
            % 记录帧
            frame = getframe(gcf);
            writeVideo(v, frame);
        end
    else
        drawMazeFlag = 0;        
    end
    % 初始步数
    steps = 0;
    % 起始状态
    maze.mazeData = mazeDataInit;
    currentState = startState;
    transitPointReward = -1;% 途经点奖励(没有奖励点)
    % 探索率、学习率调整(余弦退火)
    epsilon = epsilonMin + 0.5*(epsilonMax-epsilonMin)*(1+cos(epoch/maxEpochs*pi));
    alpha = alphaMin + 0.5*(alphaMax-alphaMin)*(1+cos(epoch/maxEpochs*pi));
    % 到达终点体力耗尽
    while steps <= maxSteps
        % 选择动作(epsilon贪心策略)  
        if rand < epsilon
            % 探索：随机选择一个动作
            action = randi(numActions);
        else  
            % 利用：选择当前状态下Q值最大的动作（如果有多个，随机选一个）  
            bestActions = find(Q(currentState, :) == max(Q(currentState, :)));
            action = bestActions(randi(length(bestActions)));
        end
        % 执行动作并观察新的状态
        [StateRow,StateCol] = ind2sub(size(maze.mazeData), currentState);
        newRow = StateRow + actions(action, 1);
        newCol = StateCol + actions(action, 2);
        % 越界控制
        if newRow <= 0 || newRow > mazeHeight || newCol <= 0 || newCol > mazeWidth
            nextState = currentState;
        else
            % 预选下一个状态  
            nextState = sub2ind(size(maze.mazeData),newRow, newCol);
            % 检查新状态是否有效
            if maze.mazeData(newRow,newCol) == 0 ||...
                (maze.mazeData(newRow,StateCol) == 0 && maze.mazeData(StateRow,newCol) == 0)
                nextState = currentState; % 碰到墙壁留在原地
            end
        end
        if drawMazeFlag && nextState ~= currentState
            maze.mazeData(ind2sub(size(maze.mazeData), nextState)) = -2;
            maze.mazeData(ind2sub(size(maze.mazeData), currentState)) = -1;
            if currentState == startState || nextState == goalState
                maze.mazeData(ind2sub(size(maze.mazeData), startState)) = 2;
                maze.mazeData(ind2sub(size(maze.mazeData), goalState)) = 3;
            else
                maze.draw();
                titleStr=sprintf("Epoch:%d---步数:%d---最佳步数:%d",epoch,steps+2,bestSteps);
                title(titleStr);
                drawnow;
            end
            if recordFlag
                % 记录帧
                frame = getframe(gcf);
                writeVideo(v, frame);
            end
        end
        % 根据是否到达目标状态来分配奖励
        if nextState == goalState && isempty(transitPointState)
            reward = 100; % 到达终点，给予大奖励 
        elseif nextState == transitPointState
            reward = transitPointReward;% 到达途经点，给予奖励
            transitPointState = [];% 到达途经一次，奖励消失
        elseif action > 4
            reward = -sqrt(2);
        else
            reward = -1; % 移动到一个新的可通行区域，给予小惩罚
        end
        % 更新Q表
        Q(currentState, action) = Q(currentState, action) + ...  
                alpha * (reward + gamma * max(Q(nextState, :)) - Q(currentState, action));
        if currentState ~= nextState
            % 步数加一
            steps = steps + 1;
        end
        currentState = nextState;
        % 如果到达终点，完成本次训练
        if currentState == goalState && isempty(transitPointState)
            if epoch > 3*maxEpochs/5
                if steps < bestSteps
                    bestSteps = steps;
                    bestQ = Q;
                end
            end
            break
        end
    end
end
if recordFlag && showFlag
    % 插入3秒的停留帧
    for i = 1:round(60*3)
        % 记录帧
        frame = getframe(gcf);
        writeVideo(v, frame);
    end
end
% 绘制最佳训练结果
pause(3);
clf;
% 起始状态
maze.mazeData = mazeDataInit;
maze.mazeMap = mazeMapInit;
maze.draw();
maze.mazeMap = [[0 0 1;0 1 1];mazeMapInit];
if recordFlag && showFlag
    % 记录帧
    frame = getframe(gcf);
    writeVideo(v, frame);
end
steps = 0;
currentState = startState;
distence = 0;
% 到达终点体力耗尽
while currentState~=goalState && steps <= maxSteps
    % 选择动作  
    bestActions = find(Q(currentState, :) == max(Q(currentState, :)));
    action = bestActions(randi(length(bestActions)));
    % 执行动作并观察新的状态
    [StateRow,StateCol] = ind2sub(size(maze.mazeData), currentState);
    newRow = StateRow + actions(action, 1);
    newCol = StateCol + actions(action, 2);
    % 越界控制
    if newRow <= 0 || newRow > mazeHeight || newCol <= 0 || newCol > mazeWidth
        nextState = currentState;
    else
        % 预选下一个状态  
        nextState = sub2ind(size(maze.mazeData),newRow, newCol);
        % 检查新状态是否有效
        if maze.mazeData(newRow,newCol) == 0 ||...
            (maze.mazeData(newRow,StateCol) == 0 && maze.mazeData(StateRow,newCol) == 0)
            nextState = currentState; % 碰到墙壁留在原地
        end
    end
    if nextState ~= currentState
        if nextState == goalState 
            maze.mazeData(ind2sub(size(maze.mazeData), nextState)) = 3;
        else
            maze.mazeData(ind2sub(size(maze.mazeData), nextState)) = -2;
            maze.mazeData(ind2sub(size(maze.mazeData), currentState)) = -1;
        end
        if currentState == startState
            maze.mazeData(ind2sub(size(maze.mazeData), currentState)) = 2;
        end
        distence = distence + norm(actions(action,:),2);
        maze.draw();
        titleStr = sprintf("最佳结果：步数:%d --- 距离%0.4f",steps+1,distence);
        title(titleStr);
        drawnow;
        if recordFlag && showFlag
            % 记录帧
            frame = getframe(gcf);
            writeVideo(v, frame);
        end
    end
    if currentState ~= nextState
        % 步数加一
        steps = steps+1;
    end
    currentState = nextState;
end
if recordFlag && showFlag
    % 插入3秒的停留帧
    for i = 1:round(60*3)
        % 记录帧
        frame = getframe(gcf);
        writeVideo(v, frame);
    end
    close(v);% 结束保存视频
end
pause(3);
close all;